

#include "speed_calculate.h"

STR_SPEED_CAL wheelSpdCal[2];
STR_LPF_HDL   speedlpf[2] = {{0.5f,0}, {0.5f,0}};
STR_LPF_HDL   anglespeedlpf = {0.2f,0};

STR_LPF_HDL   MTspeedlpf[2] = {{0.5f,0}, {0.5f,0}};


STR_ENCODER_ACCUM encoderAccum[2] = {{.motor_num = WHEEL_LIFT},{.motor_num = WHEEL_RIGHT}};


uint16_t wheel_speed_pulse_cnt[2] = {0,0};
uint16_t wheel_encoder_cnt[2] = {0,0};

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
float32_t wheel_speed_calculate(ENUM_WHEEL_NUM num)
{

	wheelSpdCal[num].pluse_now = get_encoder_pulsecnt(num);
	
	wheelSpdCal[num].pluse_erro = (int16_t)(wheelSpdCal[num].pluse_now - wheelSpdCal[num].pluse_old);
	
	if(wheelSpdCal[num].pluse_erro < 0)
	{
		wheelSpdCal[num].pluse_erro += 65535;	
	}
	
	wheelSpdCal[num].real_rps = wheelSpdCal[num].pluse_erro * WHEEL_SPEED_CAL_SF;
	
	
	wheelSpdCal[num].real_rps = signal_low_pass(&speedlpf[num], wheelSpdCal[num].real_rps);
	
	wheelSpdCal[num].pluse_old = wheelSpdCal[num].pluse_now;

	return wheelSpdCal[num].real_rps;
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

float32_t wheel_speed_calculate_rps(STR_SPEED_CAL *hdl, ENUM_WHEEL_NUM num)
{

	if(num == WHEEL_LIFT)
	{
		hdl->pluse_now = timer_counter_read(TIMER3);
	}
	else
	{
		hdl->pluse_now = timer_counter_read(TIMER1);
	}

	hdl->pluse_erro = (int16_t)(hdl->pluse_now - hdl->pluse_old);
	
	if(hdl->pluse_erro > MAGNET_ENCODER_LINE_NUM_HALF)
	{
		hdl->pluse_erro = hdl->pluse_erro - MAGNET_ENCODER_LINE_NUM;
	}
	
	else if(hdl->pluse_erro < -MAGNET_ENCODER_LINE_NUM_HALF)
	{
		hdl->pluse_erro = hdl->pluse_erro + MAGNET_ENCODER_LINE_NUM;
	}
	
	hdl->real_rps = hdl->pluse_erro * MAGNET_ENCODER_CALCULATE_SF;
	hdl->real_lpf_rps = signal_low_pass(&speedlpf[num], hdl->real_rps);
	
	
	hdl->pluse_old = hdl->pluse_now;
	
	return hdl->real_lpf_rps;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
uint16_t get_encoder_accumulate(STR_ENCODER_ACCUM *hdl)
{
	if(hdl->motor_num == WHEEL_LIFT)
	{
		hdl->eqep_now = get_eqep_pulsecnt_left();
	}
	else
	{
		hdl->eqep_now = get_eqep_pulsecnt_right();
	}
	
	hdl->eqep_erro = hdl->eqep_now - hdl->eqep_old;
	
	if(hdl->eqep_erro > MAGNET_ENCODER_LINE_NUM_HALF)
	{
		hdl->eqep_erro = hdl->eqep_erro - MAGNET_ENCODER_LINE_NUM;
	}
	
	else if(hdl->eqep_erro < -MAGNET_ENCODER_LINE_NUM_HALF)
	{
		hdl->eqep_erro = hdl->eqep_erro + MAGNET_ENCODER_LINE_NUM;
	}
	
	hdl->eqep_accum += hdl->eqep_erro;
	
	if(hdl->eqep_accum > MAGNET_ENCODER_ACCUM_MAX)
	{
		hdl->eqep_accum %= MAGNET_ENCODER_ACCUM_MAX;
	}
	else if(hdl->eqep_accum < 0)
	{
		hdl->eqep_accum = MAGNET_ENCODER_ACCUM_MAX + hdl->eqep_accum;
	}
		
	hdl->eqep_old = hdl->eqep_now;
	
	hdl->eqep_accum_sf = (uint16_t)(hdl->eqep_accum * MAGNET_ENCODER_ACCUM_SF);
	
	return hdl->eqep_accum_sf;
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
void get_wheel_speed(void)
{
	
	get_lsm6dsr_gyro_z(&userHdl.signal.angle_speed_dps_lpf);
	
	userHdl.signal.angle_speed_dps_lpf = signal_low_pass(&anglespeedlpf, userHdl.signal.angle_speed_dps_lpf);
	#if 0
	
	userHdl.signal.wheel_speed_rps[WHEEL_LIFT]  = wheel_speed_calculate(WHEEL_LIFT);
	userHdl.signal.wheel_speed_rps[WHEEL_RIGHT] = wheel_speed_calculate(WHEEL_RIGHT);
	
	#else
	
	#if 0
	userHdl.signal.wheel_speed_rps[WHEEL_LIFT]  = wheel_speed_calculate_rps(&wheelSpdCal[WHEEL_LIFT], WHEEL_LIFT);	
	userHdl.signal.wheel_speed_rps[WHEEL_RIGHT] = wheel_speed_calculate_rps(&wheelSpdCal[WHEEL_RIGHT], WHEEL_RIGHT);
	#endif
	
	float32_t speed_right_M = 0;
	float32_t speed_right_MT = 0;
		
	speed_right_M  = wheel_speed_calculate_rps(&wheelSpdCal[WHEEL_RIGHT], WHEEL_RIGHT);
	speed_right_MT = signal_low_pass(&MTspeedlpf[1], MT_calculate[1].speed_rps);
	
	if(MT_calculate[1].speed_update == 0)
	{
		userHdl.signal.wheel_speed_rps[WHEEL_RIGHT] = speed_right_M;
	}
	else
	{
		userHdl.signal.wheel_speed_rps[WHEEL_RIGHT] = speed_right_MT;		
	}
	
	MT_calculate[1].speed_update = 0;
	
	
	
	float32_t speed_left_M = 0;
	float32_t speed_left_MT = 0;
		
	speed_left_M  = wheel_speed_calculate_rps(&wheelSpdCal[WHEEL_LIFT], WHEEL_LIFT);
	speed_left_MT = signal_low_pass(&MTspeedlpf[0], MT_calculate[0].speed_rps);
	
	if(MT_calculate[0].speed_update == 0)
	{
		userHdl.signal.wheel_speed_rps[WHEEL_LIFT] = speed_left_M;
	}
	else
	{
		userHdl.signal.wheel_speed_rps[WHEEL_LIFT] = speed_left_MT;		
	}
	
	MT_calculate[0].speed_update = 0;
	
	
	
	#endif

}





